#!/usr/bin/env python3

import rospy
from lab_automation.srv import UltrasonicService, UltrasonicServiceResponse
from lab_automation.ultrasonic_device_controller import UltrasonicDeviceController

class UltrasonicServiceNode:
    def __init__(self):
        rospy.init_node('ultrasonic_service')
        self.controller = UltrasonicDeviceController()
        self.service = rospy.Service('/ultrasonic_control', UltrasonicService, self.handle_request)
        rospy.loginfo("Ultrasonic control service is ready.")

    def handle_request(self, req):
        """
        处理客户端请求
        只有当 command == 'start_ultrasonic' 时才执行
        """
        rospy.loginfo(f"Received request: command={req.command}, duration={req.duration_seconds}")

        if req.command != "start_ultrasonic":
            return UltrasonicServiceResponse(result="Invalid command. Expected 'start_ultrasonic'.")

        if req.duration_seconds < 0:
            return UltrasonicServiceResponse(result="Duration must be >= 0.")

        if req.duration_seconds == 0:
            result = self.controller.stop_ultrasonic()
        else:
            result = self.controller.start_ultrasonic(req.duration_seconds)

        return UltrasonicServiceResponse(result=result)

    def run(self):
        rospy.spin()

if __name__ == "__main__":
    node = UltrasonicServiceNode()
    try:
        node.run()
    except KeyboardInterrupt:
        rospy.loginfo("Shutting down...")
        node.controller.close()